/*
 *  Physics.cpp
 *  c_Render
 *
 *  Created by liao peterson on 11/28/09.
 *  Copyright 2009 uestc. All rights reserved.
 *
 */

#include "Physics.h"

GLint physicsIntegrator::currentID=0;
physicsIntegrator::physicsIntegrator(){
	forceRegistry = new List<force>;
	fChanged = true;
	accelerating = false;
	angAccelerating = false;
	rotating = false;
	moving = false;
	pi.uniMoment = INITIAL_INERTIA;
	transform.SetIdentity();
	pi.moment.SetIdentity();
	gravity*g = new gravity;
	pi.mass = 1;
	identity = physicsIntegrator::currentID++;
	applyForce(g);
	
	deleted = 0;
}

physicsIntegrator::~physicsIntegrator(){		//This also delete all the data inside forceRegistry
	switch (pi.boundType) {
		case BOX:
			delete pi.bBox;
			break;
		default:
			break;
	}
	delete forceRegistry;					//sth like:physicsIntegrator b= this;delete this;b is going to corrupt the memory!
}

void physicsIntegrator::applyForce(force*target){
	forceRegistry->Append(target);
	fChanged = true;
	accelerating = true;
}

void physicsIntegrator::applyImpulse(impulse*target){
	if(pi.mass<=STATIC_MASS){return;}
	pi.vel += target->f/pi.mass;
	pi.angVel += target->f.cross(target->offset)/pi.uniMoment;
	//pi.angVel += target->offset % target->f/pi.uniMoment;
}

void physicsIntegrator::applyImpulse(impulse*target, vec3&velChange,vec3&angVelChange){
	if(pi.mass<=STATIC_MASS){return;}
	velChange = target->f/pi.mass;
	angVelChange = target->f.cross(target->offset)/pi.uniMoment;
	pi.vel += velChange;
	pi.angVel += angVelChange;
	//pi.angVel += target->offset % target->f/pi.uniMoment;
	
}

// Get column vector indexing 0,1,2
vec3 physicsIntegrator::getAxis(GLint i) const{
	//if(index<0||index>2)return NULL;
	GLint index = i*4;
	vec3 temp = {transform.m[index], transform.m[index+1], transform.m[index+2]};
	temp.normalize();
	return temp;
}
/*
Matrix4x4 physicsIntegrator::inverseTransorm()
{
	Matrix4x4 t1;
	Matrix4x4 t2;
	t1.SetTranslate(-pi.locOffset.x, -pi.locOffset.y, -pi.locOffset.z);
	t2.SetRotQuatI(&pi.q);
	return (t2 * t1);
}*/

void physicsIntegrator::setMatrix(){
	Matrix4x4 t;
	Matrix4x4 rot;
	t.SetTranslate(pi.locOffset.x,
				   pi.locOffset.y,
				   pi.locOffset.z);
	pi.loc = t * target->loc;
	
	rot.SetRotQuat(&(pi.q));
	
	// the rotation comes first, translation second, to make it roatating around the right axis.
	transform = t * (*target->trans) * rot * (*target->rot) * (*target->scale); 
	//transform = t * (*target->trans) * (*target->rot) * rot * (*target->scale); 
	t.SetTranslate(-pi.locOffset.x,
				   -pi.locOffset.y,
				   -pi.locOffset.z);
	rot.SetRotQuatI(&(pi.q));
	itransform = (*target->iscale)  * (*target->irot) * rot* (*target->itrans) * t;
	//itransform = (*target->iscale)  * rot * (*target->irot) * (*target->itrans) * t;
}

void physicsIntegrator::update(){
	if(fChanged){
		vec3 foc={0,0,0};
		forceRegistry->SetCurrent(0);
		for(int i=0;i<forceRegistry->count;i++){
			forceRegistry->current->data->updateForce(pi);
			foc += forceRegistry->current->data->f;
			forceRegistry->current=forceRegistry->current->next;
		}
		pi.acc = foc/pi.mass;
		accelerating =(pi.acc.norm()>EPSILON)?true:false;		//Judge if it is accelearting
	}
	
	//if(accelerating)
	{
		pi.vel += pi.acc * DURATION;
		pi.vel *= AIR_RESTITUITION;
		pi.vel *= (pi.vel.norm()>EPSILON)?1:0;
		//moving = (pi.vel.norm()>EPSILON)?true:false;			//Judge if it is moving
	}
	
	Matrix4x4 t;
	t.SetIdentity();
	//if(moving){
	{
		pi.locOffset += pi.vel * DURATION;
		t.SetTranslate(pi.locOffset.x,
								pi.locOffset.y,
								pi.locOffset.z);
	}
	pi.loc = t * target->loc;
	
	updateTorque();
	
	Matrix4x4 rot;
	rot.SetRotQuat(&(pi.q));
	
	// the rotation comes first, translation second, to make it roatating around the right axis.
	transform = t * (*target->trans) * (*target->rot) * rot * (*target->scale); 
	t.SetTranslate(-pi.locOffset.x,
				   -pi.locOffset.y,
				   -pi.locOffset.z);
	rot.SetRotQuatI(&(pi.q));
	itransform = (*target->iscale) * rot * (*target->irot) * (*target->itrans) * t;
}

void physicsIntegrator::updateTorque(){
	if(fChanged){
		vec3 torque={0,0,0};
		forceRegistry->SetCurrent(0);
		for(int i=0;i<forceRegistry->count;i++){
			torque += ((*target->Model2World)*forceRegistry->current->data->offset-target->loc).cross(forceRegistry->current->data->f);
			forceRegistry->current = forceRegistry->current->next;
		}
	//Matrix3x3 tMoment = ((*target->Model2World) * pi.moment).Inverse();							//Shoud Transpose here? Give a simple trial first,
																									//the moment of inertia is identity?
		torque = torque/pi.uniMoment;
		
		pi.angAcc = torque;
		//angAccelerating=(pi.angAcc.norm()>EPSILON*50)?true:false;
	}
	
	//if(angAccelerating)
	{
		pi.angVel += pi.angAcc * DURATION;
		pi.angVel *= AIR_RESTITUITION;
		pi.angVel *= (pi.angVel.norm()>EPSILON)?1:0;
		//rotating = (pi.angVel.norm()>EPSILON)?true:false;
	}
	
	//if(rotating){
	{
		Quat * quat = new Quat;
		vec3 axis = pi.angVel;
		axis.normalize();
		GLfloat theta = pi.angVel.norm() * DURATION;
		quat->Set(axis, theta);
		pi.q = pi.q * (*quat);
	}
}



void force::updateForce(physicsInfo&pi){
}

void gravity::updateForce(physicsInfo&pi){
	if(!changed) return;
	if(pi.mass>STATIC_MASS){
		f.x = 0;
		f.y = 0;
		f.z = -pi.mass * GRAVITY_CONSTANT;
	}else{
		f.x = 0;
		f.y = 0;
		f.z = 0;
	}
	changed = false;
}

void physicsIntegrator::updateVel(){
	if(fChanged){
		vec3 foc={0,0,0};
		forceRegistry->SetCurrent(0);
		for(int i=0;i<forceRegistry->count;i++){
			forceRegistry->current->data->updateForce(pi);
			foc += forceRegistry->current->data->f;
			forceRegistry->current=forceRegistry->current->next;
		}
		pi.acc = foc/pi.mass;
		accelerating =(pi.acc.norm()>EPSILON)?true:false;		//Judge if it is accelearting
	}
	pi.vel += pi.acc * DURATION;
	pi.vel *= AIR_RESTITUITION;
	pi.vel *= (pi.vel.norm()>EPSILON)?1:0;
	
	pi.angVel += pi.angAcc * DURATION;
	pi.angVel *= AIR_RESTITUITION;
	pi.angVel *= (pi.angVel.norm()>EPSILON)?1:0;
}

void physicsIntegrator::updatePos(){
	Matrix4x4 t;
	t.SetIdentity();
	//if(moving){
	{
		pi.locOffset += pi.vel * DURATION;
		t.SetTranslate(pi.locOffset.x,
					   pi.locOffset.y,
					   pi.locOffset.z);
	}
	pi.loc = t * target->loc;
	
	if(fChanged){
		vec3 torque={0,0,0};
		forceRegistry->SetCurrent(0);
		for(int i=0;i<forceRegistry->count;i++){
			torque += ((*target->Model2World)*forceRegistry->current->data->offset-target->loc).cross(forceRegistry->current->data->f);
			forceRegistry->current = forceRegistry->current->next;
		}
		//Matrix3x3 tMoment = ((*target->Model2World) * pi.moment).Inverse();							//Shoud Transpose here? Give a simple trial first,
		//the moment of inertia is identity?
		torque = torque/pi.uniMoment;
		
		pi.angAcc = torque;
		//angAccelerating=(pi.angAcc.norm()>EPSILON*50)?true:false;
	}
	
	Quat * quat = new Quat;
	vec3 axis = pi.angVel;
	axis.normalize();
	GLfloat theta = pi.angVel.norm() * DURATION;
	quat->Set(axis, theta);
	pi.q = pi.q * (*quat);
	Matrix4x4 rot;
	rot.SetRotQuat(&(pi.q));
	
	transform = t * (*target->trans) * (*target->rot) * rot * (*target->scale); 
	t.SetTranslate(-pi.locOffset.x,
				   -pi.locOffset.y,
				   -pi.locOffset.z);
	rot.SetRotQuatI(&(pi.q));
	itransform = (*target->iscale) * rot * (*target->irot) * (*target->itrans) * t;
}

void physicsSimulator::update(){
	fList->SetCurrent(0);
	for(int i=0;i<fList->count;i++)
	{
		fList->current->data->setMatrix();
		fList->current=fList->current->next;
	}
	
	fList->SetCurrent(0);
	collisionData *data=new collisionData;
	for(int i=0;i<fList->count;i++)
	{
		Node<physicsIntegrator>*temp = fList->current;
		
		// See if it interacts
		if(fList->current->data->pi.boundType == STATIC || fList->current->data->pi.boundType == UNKNOWN){
			continue;
		}
		
		for(int j=i+1;j<fList->count;j++)
		{
			temp = temp->next;
			
			if(temp->data->pi.boundType == STATIC || temp->data->pi.boundType == UNKNOWN){
				continue;
			}
			
			if(fList->current->data->pi.boundType == SPHERE && temp->data->pi.boundType == SPHERE)
				colManager->Sphere2Sphere(*fList->current->data, *temp->data, data);
			if(fList->current->data->pi.boundType == BOX && temp->data->pi.boundType == BOX)
				colManager->Box2Box(*fList->current->data, *temp->data, data);
		}
		fList->current=fList->current->next;
	}
	if(data->contacts->count > 0)
	{
		//alSourcePlay(sound->source);
	}
	
	colManager->updateCache(data);
	
	fList->SetCurrent(0);
	for(int i=0;i<fList->count;i++)
	{
		fList->current->data->updateVel();
		fList->current=fList->current->next;
	}
	
	colManager->collisionResolveVel(colManager->cache);
	
	fList->SetCurrent(0);
	for(int i=0;i<fList->count;i++)
	{
		fList->current->data->updatePos();
		fList->current=fList->current->next;
	}
	
	colManager->collisionResolvePos(colManager->cache);
	
	fList->SetCurrent(0);
	for(int i=0;i<fList->count;i++)
	{
		fList->current->data->setMatrix();
		fList->current=fList->current->next;
	}
	
	//delete data;
}

GLint physicsSimulator::updateFinger(line&l)//, physicsIntegrator&target)//, GLfloat&tMin, GLint &index, GLint i)
{
	fList->SetCurrent(0);
	GLfloat tMin = BIG_VALUE;
	GLint index = -1;
	for(int i=0;i<fList->count;i++)
	{
		physicsIntegrator*temp = fList->current->data;
		if(fList->current->data->pi.boundType == BOX)
		{
			GLfloat t = tMin;
			if(colManager->Line2Box(l, *temp, tMin)){
				if(t != tMin) index = i;
				//printf("Line to box intersection detected.\n");
			}
		}
		fList->current=fList->current->next;
	}
	return index;
}
			
